#!/bin/bash

# 松灵小车硬件启动脚本

echo "==================== 松灵小车硬件启动 ===================="

# 设置环境变量
export LANG=zh_CN.UTF-8
export LC_ALL=zh_CN.UTF-8

# 检查ROS环境
if [ -z "$ROS_DISTRO" ]; then
    echo "错误: ROS环境未设置"
    echo "请运行: source /opt/ros/melodic/setup.bash"
    exit 1
fi

echo "ROS环境: $ROS_DISTRO"

# 检查catkin工作空间
if [ -z "$CATKIN_WS" ]; then
    echo "使用默认catkin工作空间: ~/catkin_ws"
    CATKIN_WS="$HOME/catkin_ws"
fi

if [ ! -d "$CATKIN_WS" ]; then
    echo "错误: catkin工作空间不存在: $CATKIN_WS"
    exit 1
fi

# 进入工作空间并source环境
cd $CATKIN_WS
source devel/setup.bash

echo "工作空间: $CATKIN_WS"
echo ""

# 启动roscore
if ! pgrep -x "roscore" > /dev/null; then
    echo "启动roscore..."
    roscore &
    sleep 3
fi

echo "roscore状态: ✓ 运行中"
echo ""

# 启动CAN通信
echo "启动CAN通信..."
sudo modprobe gs_usb
sudo ip link set can0 up type can bitrate 500000
echo "CAN通信状态: ✓ 已启动"
echo ""

# 启动底盘节点
echo "启动底盘节点..."
roslaunch scout_base scout_mini_base.launch port_name:=can0 simulated_robot:=false &
sleep 2
echo "底盘节点状态: ✓ 已启动"
echo ""

# 启动激光雷达
echo "启动激光雷达..."
roslaunch scout_bringup open_rslidar.launch &
sleep 2
echo "激光雷达状态: ✓ 已启动"
echo ""

# 启动深度相机
echo "启动深度相机..."
roslaunch realsense2_camera rs_camera.launch &
sleep 2
echo "深度相机状态: ✓ 已启动"
echo ""

# 启动IMU传感器
echo "启动IMU传感器..."
roslaunch imu_launch imu_msg.launch &
sleep 2
echo "IMU传感器状态: ✓ 已启动"
echo ""

echo "==================== 硬件启动完成 ===================="
echo "所有硬件已启动，现在可以运行监控系统："
echo "roslaunch scout_sensor_monitor scout_sensor_monitor.launch"
echo ""
echo "按 Ctrl+C 停止所有进程"
